#include "ros_udp.hpp"

bool UDP_ROS::Node_Init()
{
     //初始化发送地址
    memset(&send_addr, 0, sizeof(send_addr)); //初始化send_addr
    send_addr.sin_family = AF_INET;
    send_addr.sin_addr.s_addr = inet_addr(SERVER_IP);
    send_addr.sin_port = htons(SERVER_PORT);

    if(!UDP_ROS::Send_Message(init_pose_send_buf, sizeof(init_pose_send_buf)))
    {
        std::cout<<"Fail Send init_pose_send_buf !!!!"<<std::endl;
        return false;
    }
    if(!UDP_ROS::Send_Message(move_lock_send_buf, sizeof(move_lock_send_buf)))
    {
        std::cout<<"Fail Send move_lock_send_buf !!!!"<<std::endl;
        return false;
    }
    //std::cout<<"Init Finish!!!!"<<std::endl;
    return true;
}

bool UDP_ROS::Send_Message(unsigned char* send_buf, int send_buf_size)
{
    socklen_t addr_len = sizeof(struct sockaddr_in);
    unsigned char send_buf_temp[send_buf_size] = {0};
    for(int i = 0;i<send_buf_size;i++)
    {
        send_buf_temp[i] = send_buf[i];
        //std::cout<<(int)send_buf_temp[i]<<" ";
    }
    //通讯开始
    if(sendto(sockfd, send_buf_temp, sizeof(send_buf_temp), 0, (struct sockaddr *)&send_addr,addr_len) < 0)
    //发送失败 
    {
        std::cout<<"send message failed.."<<std::endl;
        return false;
    }
    //发送结束
    return true;
}

int UDP_ROS::Get_message(unsigned char* recv_buf)
{
    socklen_t addr_len = sizeof(struct sockaddr_in);
    int recv_len = recvfrom(sockfd, recv_buf, RECV_BUUF_SIZE, 0, (struct sockaddr *)&recv_addr, &addr_len);
    //std::cout<<recv_len<<std::endl;
    if (recv_len < 0) {
        std::cout<<" message length <0 "<<std::endl;
        return -1;
    }
    return recv_len;
}

std::vector<double> UDP_ROS::Deserialize_msg(int sockfd)
{
    std::vector<double> payload_msg = {-1,0,0,0,0,0};

    //发送接收指令
    unsigned char recv_buf[RECV_BUUF_SIZE] = {0};
    int recv_len = UDP_ROS::Get_message(recv_buf);
    std::cout << "接收到一个信息： " << recv_buf << std::endl;
    //数据CRC16检查
    if(!CRC16_Check(recv_buf, recv_len)) 
    {
        std::cout<<"CRC Check failed.."<<std::endl;
        return payload_msg;
    }
    //读取数据信息
    
    


    return payload_msg; 
}

bool UDP_ROS::CRC16_Check(unsigned char* message_char, int len)
{
    uint16_t crc = UDP_ROS::CRC16_Cal(message_char,len);
    uint16_t crc_check = (message_char[len-2] << 0) | (message_char[len-1] << 8);
    if(crc_check != crc) return false;
    //printf("%04x ", crc_check);
    return true;
}

uint16_t UDP_ROS::CRC16_Cal(unsigned char* message_char, int len)
{
    uint16_t crc, oldcrc16;
    uint8_t temp;
    crc = 0x0000;
    int i = 0;
    while (i < len - 2) //去掉已知校验位
    {
        temp=(crc>>8)&0xff;
        oldcrc16=crc16_tab[message_char[i]^temp];
        crc=(crc<<8)^oldcrc16;
        i++;
    }
    return crc;
}

bool UDP_ROS::Change_Pose(std::vector<double> set_pose)
{
    //初始化命令
    unsigned char send_pose[14] = 
        {0x55,0x66,0x01,0x04,0x00,0x00,0x00,0x0E,0x00,0x00,0x00,0x00,0x00,0x00}; //补充CRC16校验
    std::cout<<set_pose[0]<<" "<<set_pose[1]<<std::endl;
    //将设置姿态转化为unsign char表示
    int16_t set_yaw_int16 = (int16_t)(set_pose[0]*10.0);
    //if(set_pose[0] < 0.0)    set_yaw_uint16 = (uint16_t)(set_pose[0]*10 + 32768);
    //else    set_yaw_uint16 = (uint16_t)(set_pose[0]*10);
    //yaw
    std::pair<unsigned char, unsigned char> set_yaw_char = UDP_ROS::Int_2uint8(set_yaw_int16);
    send_pose[8] = set_yaw_char.first;
    send_pose[9] = set_yaw_char.second;

    int16_t set_pitch_int16 = (int16_t)(set_pose[1]*10.0);
    //if(set_pose[1] < 0.0)    set_pitch_uint16 = (int16_t)(set_pose[1]*10 + 32768);
    //else    set_pitch_uint16 = (int16_t)(set_pose[1]*10);
    //pitch
    std::pair<unsigned char, unsigned char> set_pitch_char = UDP_ROS::Int_2uint8(set_pitch_int16);
    send_pose[10] = set_pitch_char.first;
    send_pose[11] = set_pitch_char.second;
    // printf("%02x ", send_pose[10]);
    // printf("%02x ", send_pose[11]);
    //计算CRC16编码
    uint16_t crc_send = UDP_ROS::CRC16_Cal(send_pose, sizeof(send_pose));
    std::pair<unsigned char, unsigned char> set_crc_char = UDP_ROS::Int_2uint8(crc_send);
    send_pose[12] = set_crc_char.first;
    send_pose[13] = set_crc_char.second;

    //std::cout<<"1"<<std::endl;
    // for(int i = 0; i<sizeof(send_pose); i++)
    // {
    //     printf("%02x ", send_pose[i]);
    // }
    // std::cout<<std::endl;
    //发送消息
    if(!UDP_ROS::Send_Message(send_pose, sizeof(send_pose)))
    {
        std::cout<<"Fail Send init_pose_send_buf !!!!"<<std::endl;
        return false;
    }
    return true;
}

std::pair<unsigned char, unsigned char> UDP_ROS::Int_2uint8(int16_t num)
{
    std::pair<unsigned char, unsigned char> pose_char = std::make_pair(
                   (uint8_t)((num & 0x00ff) >> 0) ,
                   (uint8_t)((num & 0xff00) >> 8)); 
    return pose_char;
}
